#!/usr/bin/env python
# coding = utf-8
import rospy
from sensor_msgs.msg import LaserScan
from ackermann_msgs.msg import AckermannDriveStamped

cmd_vel_pub = rospy.Publisher(
        "/vesc/low_level/ackermann_cmd_mux/input/teleop", AckermannDriveStamped, queue_size=1)
ack = AckermannDriveStamped()
flag = 1
def scan_callback(msg):
    
    range_ahead = msg.ranges[len(msg.ranges)/2]
    print "range ahead: %0.3f" % range_ahead
    range_left = msg.ranges[len(msg.ranges)*5/6]
    #print "range_left: %0.3f" % range_left
    range_right = msg.ranges[len(msg.ranges)/6]
    print "range_right: %0.3f" % range_right
    speed = 0.28 * min(range_ahead,2) - 0.20
    # speed = 0.276 * range_ahead - 0.207
    if range_ahead < 1.2:
        flag = 0
        speed = 0

    ang = 1.03 - range_right
    # ang = 1 - range_right

    ack.drive.speed = speed
    ack.drive.steering_angle = ang
    cmd_vel_pub.publish(ack)
    print("N/A. speed = %.1f angle = %.2f"%(speed,ang))
    rospy.sleep(0.2)
    

def main():
    
    scan_sub = rospy.Subscriber('/scan', LaserScan, scan_callback)    
    if flag == 1:
        rospy.spin()

if __name__ == "__main__":
    rospy.init_node('range_ahead')
    main()
    

